

# Control history generator


import matplotlib.pyplot as plt
import numpy as np
import csv
import pid_simples
import sys
import math

from models import *

import embetant

Gpid = pid_simples.PID (0.0 , 0.0 , 0.0)


Gerro    = 0.0
Gidxctrl = 0

Gctrl = 0.0



#----------------------------------------------------
def mul (num,v):
   return [valor * num for valor in v]

#----------------------------------------------------
def add (v1,v2):
   n = len(v1)
   v = []
   for k in range(n):
      v.append (v1[k] + v2[k])
   return v

#----------------------------------------------------
def RK (f, t, dt, s) :

#   t = 0
   one6 = 1./6.
   one3 = 1./3.
   h = dt
   t0 = t
   half    = h * 0.5
   t_h     = t0 + h
   t_half  = t0 + half

   v1 = mul (h , f (t0, s))
   vt = add (v1, mul (0.5, v1))

   v2 = mul (h , f (t_half, vt))
   vt = add (v2, mul (0.5, v1))

   v3 = mul (h , f (t_half, vt))
   vt = add (s, v3)

   v4 = mul (h , f (t_h   , vt))

   vt = add (s, \
          add( mul (one6, v1), \
            add( mul (one3, v2), \
              add (mul (one3, v3), mul (one6, v4)))))

   return vt




#----------------------------------------------------


#Read PID parameters (from nomad)
try:
    f = open(sys.argv[1], "r")
    valctrl = f.read().split()

    Gkp = float(valctrl[0])
    Gki = float(valctrl[1])
    Gkd = float(valctrl[2])

    Gpid.coeficientes (Gkp, Gki, Gkd)

    flag_running_nomad = True


except:
    flag_running_nomad = False



if not flag_running_nomad:
    try:
        f = open("otimo.csv", "r")
        valctrl = f.read().split()

        Gkp = float(valctrl[2])
        Gki = float(valctrl[3])
        Gkd = float(valctrl[4])

        Gpid.coeficientes (Gkp, Gki, Gkd)
    except:
        Gkp , Gki , Gkd = 0.00158366 , 0 , 1



# Read reference trajectory  -------------------------------------------
try:
    arq  = open('Reference.csv', 'r')

    line = arq.readline()
    GRef_x = [float (x) for x in line.split(',')]

    line = arq.readline()
    GRef_y = [float (y) for y in line.split(',')]

except:
    print ("Impossível ler Trajetória de Referência, Reference.csv!")
    sys.exit (-1)



dt = 1.0 / 256.0

s  = [10., 10.]
vx = [10.0]
vy = [10.0]

Gctrl        = 0
embetant.Ghistoryctrl = [Gctrl]



for t in range(10000):
    o = RK (LotkaVolterraPID, t, dt, s)
    vx.append(o[0])
    vy.append(o[1])
    s = o

    Gctrl = Gpid.calcule (o[1] - GRef_y[t])

    embetant.Ghistoryctrl.append (Gctrl)

    # Early abort (le cas où...).
    if (o[0] > 100) or (o[1] > 100):
       print (1e9)
       sys.exit(0)




#-----------------------------------------------
n = len(GRef_y)
erro = 0

for idx in range(n):
    e = vy[idx] - GRef_y[idx]
    erro = erro + (e*e)

print (math.sqrt(erro))

    
# If nomad is running, we're done.
if flag_running_nomad:
    sys.exit (0)


# Nomad is not running; let's record the control history.
if len(embetant.Ghistoryctrl) > 0:

    with open("Ctrl.csv", 'wt') as myfile:
        wr = csv.writer(myfile)
        wr.writerow(embetant.Ghistoryctrl)

    print ('len(Ghistoryctrl) = ', len(embetant.Ghistoryctrl))




